#include "ti_msp_dl_config.h"
#include <math.h>
#include "mpu6050.h"
#include <ti/drivers/dpl/ClockP.h>
#include <ti/drivers/I2C.h>

#include "stdio.h"
#include "targetConfigs/Hardware/uart.h"
#include "E:\CCS\mspm0_sdk_2_05_01_00\source\ti\driverlib\dl_i2c.h"


#define RAD_TO_DEG 57.295779513082320876798154814105  // 弧度转角度系数（180/π）
#define WHO_AM_I_REG 0x75      // MPU6050设备ID寄存器
#define PWR_MGMT1_REG			0X6B	//电源管理寄存器1
#define PWR_MGMT2_REG			0X6C	//电源管理寄存器2 
#define SMPLRT_DIV_REG 0x19    // 采样率分频寄存器
#define ACCEL_CONFIG_REG 0x1C  // 加速度计量程配置寄存器
#define ACCEL_XOUT_H_REG 0x3B  // 加速度计X轴数据寄存器（高字节）
#define TEMP_OUT_H_REG 0x41    // 温度数据寄存器
#define GYRO_CONFIG_REG 0x1B   // 陀螺仪量程配置寄存器
#define GYRO_XOUT_H_REG 0x43   // 陀螺仪X轴数据寄存器（高字节）
#define MPU6050_ADDR 0xD0      // MPU6050的I2C地址（7位地址0x68左移1位）

#define MPU6050_I2C_INSTANCE  0
// Setup MPU6050
// #define MPU6050_ADDR 0x68
const uint16_t i2c_timeout = 100;         // I2C通信超时时间（ms）
const double Accel_Z_corrector = 14418.0; // Z轴加速度校准系数（用于修正误差）
uint32_t timer;                           // 计时器，用于计算时间间隔

Kalman_t KalmanX = {
    .Q_angle = 0.001f,
    .Q_bias = 0.003f,
    .R_measure = 0.03f};

Kalman_t KalmanY = {
    .Q_angle = 0.001f,
    .Q_bias = 0.003f,
    .R_measure = 0.03f,
};

/**
 * @brief 向MPU6050写入数据
 * @param reg 寄存器地址
 * @param data 要写入的数据
 * @return 0表示成功，非0表示失败
 */
int MPU6050_Write(uint8_t reg, uint8_t data) {

    DL_I2C_flushControllerTXFIFO(mpu6050_INST);
    DL_I2C_flushControllerRXFIFO(mpu6050_INST);

    uint8_t buf[2] = {reg, data};
    DL_I2C_fillControllerTXFIFO(mpu6050_INST, buf,2);
    while (! (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));

    DL_I2C_startControllerTransfer(mpu6050_INST, MPU6050_ADDR >> 1, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
    while (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
	while (!(DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE)){}

    DL_I2C_startControllerTransfer(mpu6050_INST, MPU6050_ADDR >> 1, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
    while (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
	while (!(DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE)){}

    return 0;
}

/**
 * @brief 从MPU6050读取数据
 * @param reg 要读取的寄存器地址
 * @return 读取到的数据，如果读取失败返回0xFF
 */
uint8_t MPU6050_Read(uint8_t reg) {
    uint8_t data = 0xFF;
    
    // 清空FIFO
    DL_I2C_flushControllerTXFIFO(mpu6050_INST);
    DL_I2C_flushControllerRXFIFO(mpu6050_INST);

    // 发送寄存器地址
    DL_I2C_fillControllerTXFIFO(mpu6050_INST, &reg, 1);
    while (!(DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));

    // 启动写传输
    DL_I2C_startControllerTransfer(mpu6050_INST, MPU6050_ADDR >> 1, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
    while (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
    while (!(DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE)) {}

    // 启动读传输
    DL_I2C_startControllerTransfer(mpu6050_INST, MPU6050_ADDR >> 1, DL_I2C_CONTROLLER_DIRECTION_RX, 1);
    while (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
    while (!(DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE)) {}

    // 获取数据
    data = DL_I2C_receiveControllerData(mpu6050_INST);
    DL_I2C_flushControllerTXFIFO(mpu6050_INST);
    
    return data;
}

// MSPM0 DL库的MPU6050初始化
uint8_t MPU6050_Init(void) {
    // uint8_t check =1;
    uint8_t data;
    uint8_t regAddr = WHO_AM_I_REG;
    
/************************/
//     DL_I2C_flushControllerTXFIFO(mpu6050_INST);
//     DL_I2C_flushControllerRXFIFO(mpu6050_INST);

//     uint8_t tx_buf[3] = {WHO_AM_I_REG, WHO_AM_I_REG, 0x55};
//     DL_I2C_fillControllerTXFIFO(mpu6050_INST, tx_buf, 1);
//     while (! (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));

//     DL_I2C_startControllerTransfer(mpu6050_INST, MPU6050_ADDR >> 1, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
//     while (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
// 	while (!(DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE)){}

//     DL_I2C_startControllerTransfer(mpu6050_INST, MPU6050_ADDR >> 1, DL_I2C_CONTROLLER_DIRECTION_RX, 1);//读
//     while (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
// 	while (!(DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE)){}

//     check = DL_I2C_receiveControllerData(mpu6050_INST);
//     DL_I2C_flushControllerTXFIFO(mpu6050_INST);
//     // printf("ID:%d\r\n",check);
// /************************/
//     if (check != 0x68) {
//         return 1; // 设备ID不匹配
//     }
    

//     /* 唤醒传感器 (PWR_MGMT_1_REG) */

//     DL_I2C_flushControllerTXFIFO(mpu6050_INST);
//     DL_I2C_flushControllerRXFIFO(mpu6050_INST);

//     uint8_t buf[2] = {PWR_MGMT_1_REG,0x00};
//     DL_I2C_fillControllerTXFIFO(mpu6050_INST, buf,2);
//     while (! (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));

//     DL_I2C_startControllerTransfer(mpu6050_INST, MPU6050_ADDR >> 1, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
//     while (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
// 	while (!(DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE)){}

//     DL_I2C_startControllerTransfer(mpu6050_INST, MPU6050_ADDR >> 1, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
//     while (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
// 	while (!(DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE)){}

//     // return 0;

//     /* 设置采样率 (SMPLRT_DIV_REG) */
//     DL_I2C_flushControllerTXFIFO(mpu6050_INST);
//     DL_I2C_flushControllerRXFIFO(mpu6050_INST);

//     buf[0] = SMPLRT_DIV_REG;
//     buf[1] = 0x07;
//     DL_I2C_fillControllerTXFIFO(mpu6050_INST, buf,2);
//     while (! (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));

//     DL_I2C_startControllerTransfer(mpu6050_INST, MPU6050_ADDR >> 1, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
//     while (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
// 	while (!(DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE)){}

//     DL_I2C_startControllerTransfer(mpu6050_INST, MPU6050_ADDR >> 1, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
//     while (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
// 	while (!(DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE)){}
    
//     /* 配置加速度计 (ACCEL_CONFIG_REG) - ±2g范围 */
//     DL_I2C_flushControllerTXFIFO(mpu6050_INST);
//     DL_I2C_flushControllerRXFIFO(mpu6050_INST);

//     buf[0] = ACCEL_CONFIG_REG;buf[1] = 0x00;
//     DL_I2C_fillControllerTXFIFO(mpu6050_INST, buf,2);
//     while (! (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));

//     DL_I2C_startControllerTransfer(mpu6050_INST, MPU6050_ADDR >> 1, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
//     while (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
// 	while (!(DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE)){}

//     DL_I2C_startControllerTransfer(mpu6050_INST, MPU6050_ADDR >> 1, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
//     while (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
// 	while (!(DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE)){}
    
//     /* 配置陀螺仪 (GYRO_CONFIG_REG) - ±250°/s范围 */
//     DL_I2C_flushControllerTXFIFO(mpu6050_INST);
//     DL_I2C_flushControllerRXFIFO(mpu6050_INST);

//     buf[0] = GYRO_CONFIG_REG;buf[1] = 0x00;
//     DL_I2C_fillControllerTXFIFO(mpu6050_INST, buf,2);
//     while (! (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));

//     DL_I2C_startControllerTransfer(mpu6050_INST, MPU6050_ADDR >> 1, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
//     while (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
// 	while (!(DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE)){}

//     DL_I2C_startControllerTransfer(mpu6050_INST, MPU6050_ADDR >> 1, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
//     while (DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
// 	while (!(DL_I2C_getControllerStatus(mpu6050_INST) & DL_I2C_CONTROLLER_STATUS_IDLE)){}
    
    // return 0; // 初始化成功
// 检查设备ID
    uint8_t check = MPU6050_Read(WHO_AM_I_REG);
    if (check != 0x68) {
        return 1; // 设备ID不匹配
    }
    MPU6050_Write(PWR_MGMT1_REG, 0x80);
    delay_cycles(100);
    // 唤醒传感器 (PWR_MGMT_1_REG)
    MPU6050_Write(PWR_MGMT1_REG, 0x00);

    // 设置采样率 (SMPLRT_DIV_REG)
    MPU6050_Write(SMPLRT_DIV_REG, 0x07);

    // 配置加速度计 (ACCEL_CONFIG_REG) - ±2g范围
    MPU6050_Write(ACCEL_CONFIG_REG, 0x00);

    // 配置陀螺仪 (GYRO_CONFIG_REG) - ±250°/s范围
    MPU6050_Write(GYRO_CONFIG_REG, 0x00);

    MPU6050_Write(PWR_MGMT1_REG, 0x01);
    MPU6050_Write(PWR_MGMT2_REG, 0x00);
    return 0;
}

void MPU6050_Read_Accel(MPU6050_t *DataStruct) {
    uint8_t reg_addr = ACCEL_XOUT_H_REG;      // 要读取的起始寄存器地址 0x3B end:0x40
    uint8_t recv_data[6];                     // 存储读取的6字节数据

   for (int i = 0;i < 6;i++)
   {
     recv_data[i] = MPU6050_Read(reg_addr + i);
   }

    // 解析原始数据（高位在前，低位在后）
    DataStruct->Accel_X_RAW = (int16_t)(recv_data[0] << 8 | recv_data[1]);
    DataStruct->Accel_Y_RAW = (int16_t)(recv_data[2] << 8 | recv_data[3]);
    DataStruct->Accel_Z_RAW = (int16_t)(recv_data[4] << 8 | recv_data[5]);

    // 转换为加速度值（g），假设 FS_SEL=0（量程 ±2g，灵敏度 16384 LSB/g）
    DataStruct->Ax = DataStruct->Accel_X_RAW / 16384.0f;
    DataStruct->Ay = DataStruct->Accel_Y_RAW / 16384.0f;
    DataStruct->Az = DataStruct->Accel_Z_RAW / 16384.0f;  // 若需校准，替换分母
}

void MPU6050_Read_Gyro(MPU6050_t *DataStruct) {
    uint8_t reg_addr = GYRO_XOUT_H_REG;      // 要读取的起始寄存器地址 0x43 end:0x48
    uint8_t recv_data[6];                     // 存储读取的6字节数据

   for (int i = 0;i < 5;i++)
   {
     recv_data[i] = MPU6050_Read(reg_addr + i);
     printf("%d\r\n",recv_data[i]);
   }

     // 组合成16位有符号整数
    DataStruct->Gyro_X_RAW = (int16_t)(recv_data[0] << 8 | recv_data[1]);
    DataStruct->Gyro_Y_RAW = (int16_t)(recv_data[2] << 8 | recv_data[3]);
    DataStruct->Gyro_Z_RAW = (int16_t)(recv_data[4] << 8 | recv_data[5]);

    // 转换为度/秒(±250°/s量程下灵敏度为131 LSB/°/s)
    DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0;
    DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0;
    DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0;
}

void MPU6050_Read_Temp(MPU6050_t *DataStruct) {
    uint8_t reg_addr = TEMP_OUT_H_REG;      // 要读取的起始寄存器地址 0x41 end:0x42
    uint8_t recv_data[2];                     // 存储读取的6字节数据
    int16_t temp;         // 临时存储温度值

   for (int i = 0;i < 2;i++)
   {
     recv_data[i] = MPU6050_Read(reg_addr + i);
   }


    // 组合成16位有符号整数
    temp = (int16_t)(recv_data[0] << 8 | recv_data[1]);
    
    // 转换为摄氏度(根据MPU6050手册提供的公式)
    DataStruct->Temperature = (float)((int16_t)temp / (float)340.0 + (float)36.53);
}

// void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
// {
//     uint8_t Rec_Data[14];  // 存储14字节数据(加速度6+温度2+陀螺仪6)
//     int16_t temp;          // 临时存储温度值

//     // 一次性读取14字节数据(从ACCEL_XOUT_H开始)
//     HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 14, i2c_timeout);

//     // 解析加速度数据(字节0-5)
//     DataStruct->Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
//     DataStruct->Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
//     DataStruct->Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
    
//     // 解析温度数据(字节6-7)
//     temp = (int16_t)(Rec_Data[6] << 8 | Rec_Data[7]);
    
//     // 解析陀螺仪数据(字节8-13)
//     DataStruct->Gyro_X_RAW = (int16_t)(Rec_Data[8] << 8 | Rec_Data[9]);
//     DataStruct->Gyro_Y_RAW = (int16_t)(Rec_Data[10] << 8 | Rec_Data[11]);
//     DataStruct->Gyro_Z_RAW = (int16_t)(Rec_Data[12] << 8 | Rec_Data[13]);

//     // 转换为实际物理量
//     DataStruct->Ax = DataStruct->Accel_X_RAW / 16384.0;  // X轴加速度(g)
//     DataStruct->Ay = DataStruct->Accel_Y_RAW / 16384.0;  // Y轴加速度(g)
//     DataStruct->Az = DataStruct->Accel_Z_RAW / Accel_Z_corrector;  // Z轴加速度(g)
//     DataStruct->Temperature = (float)((int16_t)temp / (float)340.0 + (float)36.53);  // 温度(℃)
//     DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0;  // X轴角速度(°/s)
//     DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0;  // Y轴角速度(°/s)
//     DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0;  // Z轴角速度(°/s)

//     // 卡尔曼滤波计算角度
//     double dt = (double)(HAL_GetTick() - timer) / 1000;  // 计算时间间隔(秒)
//     timer = HAL_GetTick();  // 更新计时器
    
//     // 计算滚转角(roll)
//     double roll;
//     double roll_sqrt = sqrt(DataStruct->Accel_X_RAW * DataStruct->Accel_X_RAW + 
//                            DataStruct->Accel_Z_RAW * DataStruct->Accel_Z_RAW);
//     if (roll_sqrt != 0.0) {
//         roll = atan(DataStruct->Accel_Y_RAW / roll_sqrt) * RAD_TO_DEG;
//     } else {
//         roll = 0.0;
//     }
    
//     // 计算俯仰角(pitch)
//     double pitch = atan2(-DataStruct->Accel_X_RAW, DataStruct->Accel_Z_RAW) * RAD_TO_DEG;
    
//     // Y轴卡尔曼滤波(处理角度突变情况)
//     if ((pitch < -90 && DataStruct->KalmanAngleY > 90) || (pitch > 90 && DataStruct->KalmanAngleY < -90)) {
//         KalmanY.angle = pitch;
//         DataStruct->KalmanAngleY = pitch;
//     } else {
//         DataStruct->KalmanAngleY = Kalman_getAngle(&KalmanY, pitch, DataStruct->Gy, dt);
//     }
    
//     // 当俯仰角超过±90°时，反转陀螺仪X轴数据
//     if (fabs(DataStruct->KalmanAngleY) > 90)
//         DataStruct->Gx = -DataStruct->Gx;
    
//     // X轴卡尔曼滤波
//     DataStruct->KalmanAngleX = Kalman_getAngle(&KalmanX, roll, DataStruct->Gx, dt);
// }

// double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt)
// {
//     double rate = newRate - Kalman->bias;
//     Kalman->angle += dt * rate;

//     Kalman->P[0][0] += dt * (dt * Kalman->P[1][1] - Kalman->P[0][1] - Kalman->P[1][0] + Kalman->Q_angle);
//     Kalman->P[0][1] -= dt * Kalman->P[1][1];
//     Kalman->P[1][0] -= dt * Kalman->P[1][1];
//     Kalman->P[1][1] += Kalman->Q_bias * dt;

//     double S = Kalman->P[0][0] + Kalman->R_measure;
//     double K[2];
//     K[0] = Kalman->P[0][0] / S;
//     K[1] = Kalman->P[1][0] / S;

//     double y = newAngle - Kalman->angle;
//     Kalman->angle += K[0] * y;
//     Kalman->bias += K[1] * y;

//     double P00_temp = Kalman->P[0][0];
//     double P01_temp = Kalman->P[0][1];

//     Kalman->P[0][0] -= K[0] * P00_temp;
//     Kalman->P[0][1] -= K[0] * P01_temp;
//     Kalman->P[1][0] -= K[1] * P00_temp;
//     Kalman->P[1][1] -= K[1] * P01_temp;

//     return Kalman->angle;
// };